#include "mbot_linux_serial.h"
#include <serial/serial.h>
#include <iostream>

serial::Serial sp;//创建一个serial对象

const unsigned char ender[2] = {0x0d, 0x0a};
const unsigned char header[2] = {0x55, 0xaa};

//发送左右轮速控制速度共用体
union sendData
{
	short d;
	unsigned char data[2];
};
union sendData angle[7];
//接收数据（左轮速、右轮速、角度）共用体（-32767 - +32768）
union receiveData
{
	short d;
	unsigned char data[2];
}leftVelNow,rightVelNow,angleNow;


// 函数功能：串口参数初始化

void Serial_Init()
{
    
    serial::Timeout to = serial::Timeout::simpleTimeout(100);//创建timeout
	serial::parity_t pt = serial::parity_t::parity_none;//创建校验位为0位
	serial::bytesize_t bt = serial::bytesize_t::eightbits;//创建发送字节数为8位
	serial::flowcontrol_t ft = serial::flowcontrol_t::flowcontrol_none;//创建数据流控制，不使用
	serial::stopbits_t st = serial::stopbits_t::stopbits_one;//创建终止位为1位
    
    sp.setPort("/dev/ttyTHS0");//设置要打开的串口名称
    sp.setBaudrate(115200);//设置串口通信的波特率
    sp.setParity(pt);//设置校验位
    sp.setBytesize(bt);//设置发送字节数
    sp.setFlowcontrol(ft);//设置数据流控制
    sp.setStopbits(st);//设置终止位
    
    sp.setTimeout(to);//串口设置timeout
 
    try
    {
        //打开串口
        sp.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return;
    }
}

void writeSpeed(int* p, unsigned char ctrlFlag)
{
    unsigned char buf[20] = {0};//
    int i, length = 0;

    // angle.d  = *p;//mm/s
    // rightVelSet.d = Right_v;
     for (int i = 0; i < 7; i++) {
        angle[i].d = p[i];
     }
    //  ROS_INFO("data:check:%d,%d,%d,%d,%d",p[0],p[1],p[2],p[3],p[4]);
    // 设置消息头
    for(i = 0; i < 2; i++)
        buf[i] = header[i];             //buf[0]  buf[1]
    // 设置机器人左右轮速度
    length = 11;
    buf[2] = length;                    //buf[2]
    // ROS_INFO("header check:%x,%x",buf[0],buf[1]);
    for(i = 0; i < 2; i++)
    {
        buf[i + 3] = angle[0].data[i];  //buf[3] buf[4]
        buf[i + 5] = angle[1].data[i]; //buf[5] buf[6]
        buf[i + 7] = angle[2].data[i];//7、8
        buf[i + 9] = angle[3].data[i];//9、10
        buf[i + 11] = angle[4].data[i];//11、12
        // buf[i + 13] = angle[5].data[i];//13、14
    }
    // ROS_INFO("0X:%x,%x,%x,%x,%x,%x,%x,%x,%x,%x",buf[3],buf[4],buf[5],buf[6],buf[7],buf[8],buf[9],buf[10],buf[11],buf[12]);
    // 预留控制指令
    buf[3 + length - 1] = ctrlFlag;       //buf[13]

    // 设置校验值、消息尾
    buf[3 + length] = getCrc8(buf, 3 + length);//buf[14]
    buf[3 + length + 1] = ender[0];     //buf[15]
    buf[3 + length + 2] = ender[1];     //buf[16]

    // 通过串口下发数据
    sp.write(buf, 20);
    // ros::Duration(1).sleep();//延时函数单位是秒
}

bool readSpeed(unsigned char &checkFlag, unsigned char &statusInfo, unsigned char &errorInfo, 
               int *speedValues, int *positionValues)
{
    char i, length = 0;
    unsigned char checkSum;
    unsigned char buf[150] = {0};
    int n;

    size_t Receive_N = sp.available(); 
    // 打印 Receive_N
    // std::cout << "Available bytes: " << Receive_N << std::endl;
    //Receive_N就是收到的字节长度

        n = sp.read(buf, Receive_N); // 读出数据    
        // 打印读到的原始数据
    if (Receive_N == 27 && buf[0]!=0 && buf[26]!=0)
    {
        std::stringstream ss;
        for (size_t j = 0; j < n; ++j) {
        ss << std::hex << static_cast<int>(buf[j]) << " ";
        }
        // ROS_INFO_STREAM("Raw Data Received: " << ss.str());
        // std::cout << std::endl; // 添加换行

        // 检查信息头
        if (buf[0] != header[0] || buf[1] != header[1]) // buf[0] buf[1]
        {
            ROS_ERROR("head error");
            return false;
        }

        // 数据长度
        length = buf[2]; // buf[2]

        // 检查信息校验值
        checkSum = getCrc8(buf, 3 + length); // buf[10] 计算得出
        if (checkSum != buf[3 + length])     // buf[10] 串口接收
        {
            // ROS_ERROR("crc error");
            return false;
        }

        checkFlag = buf[3];
        statusInfo = buf[4];
        errorInfo = buf[5];

        // 读取速度值
        for (i = 0; i < 5; i++)
        {
            speedValues[i] = (int16_t)((buf[6 + i * 2] << 8) | buf[7 + i * 2]);
        }

        // 读取位置值
        for (i = 0; i < 5; i++)
        {
            positionValues[i] = (int16_t)((buf[16 + i * 2] << 8) | buf[17 + i * 2]);
        }

        return true;
    }
    return false;
}

unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
    unsigned char crc;
    unsigned char i;
    crc = 0;
    while(len--)
    {
        crc ^= *ptr++;
        for(i = 0; i < 8; i++)
        {
            if(crc&0x01)
                crc=(crc>>1)^0x8C;
            else 
                crc >>= 1;
        }
    }
    return crc;
}
